import numpy as np
import rospy
from tf.transformations import euler_from_matrix, quaternion_from_euler, euler_matrix, quaternion_from_matrix

def deg2rad(deg):
    return deg * np.pi / 180. 

def rad2deg(rad):
    return rad / np.pi * 180. 


roll = deg2rad(30)
pitch = deg2rad(45)
yaw = deg2rad(60)

# 使用欧拉角计算四元数 内旋xyz
quat = quaternion_from_euler(roll, pitch, yaw, axes='sxyz')

# 打印四元数
print(quat)

# 外旋
quat = quaternion_from_euler(yaw, pitch, roll, axes='rzyx')
print(quat)

quat = quaternion_from_euler(yaw, pitch, roll, axes='szyx')
print(quat)

# 内旋 r表示外旋，s表示内旋
rotation_matrix = euler_matrix(yaw, pitch, roll, axes='rzyx')

print(rotation_matrix)
# 从旋转矩阵计算四元数
quat = quaternion_from_matrix(rotation_matrix)

# 打印四元数
print(quat)

# 默认axes='sxyz'内旋
eu = euler_from_matrix(rotation_matrix, axes='sxyz')
print(rad2deg(eu[0]), rad2deg(eu[1]), rad2deg(eu[2]))

# 外旋zyx
eu = euler_from_matrix(rotation_matrix, axes='rzyx')
print(rad2deg(eu[0]), rad2deg(eu[1]), rad2deg(eu[2]))
